
' ************************************************************************************
' File:        EC_QNDC.lib
' Purpose:     to configure ethercat for QNDC drive
' Version:     1.01
' Author:      Rocky Qiu
' Description: 
' History:     2021-04-09  V1.00 Created
'              2021-07-13  V1.01 add PDO mapping
                                                                                
' *************************************************************************************                                                                                    
'      QQQQQQQQQ     NNNNNNNN        NNNNNNNNDDDDDDDDDDDDD                CCCCCCCCCCCCC
'    QQ:::::::::QQ   N:::::::N       N::::::ND::::::::::::DDD          CCC::::::::::::C
'  QQ:::::::::::::QQ N::::::::N      N::::::ND:::::::::::::::DD      CC:::::::::::::::C
' Q:::::::QQQ:::::::QN:::::::::N     N::::::NDDD:::::DDDDD:::::D    C:::::CCCCCCCC::::C
' Q::::::O   Q::::::QN::::::::::N    N::::::N  D:::::D    D:::::D  C:::::C       CCCCCC
' Q:::::O     Q:::::QN:::::::::::N   N::::::N  D:::::D     D:::::DC:::::C              
' Q:::::O     Q:::::QN:::::::N::::N  N::::::N  D:::::D     D:::::DC:::::C              
' Q:::::O     Q:::::QN::::::N N::::N N::::::N  D:::::D     D:::::DC:::::C              
' Q:::::O     Q:::::QN::::::N  N::::N:::::::N  D:::::D     D:::::DC:::::C              
' Q:::::O     Q:::::QN::::::N   N:::::::::::N  D:::::D     D:::::DC:::::C              
' Q:::::O  QQQQ:::::QN::::::N    N::::::::::N  D:::::D     D:::::DC:::::C              
' Q::::::O Q::::::::QN::::::N     N:::::::::N  D:::::D    D:::::D  C:::::C       CCCCCC
' Q:::::::QQ::::::::QN::::::N      N::::::::NDDD:::::DDDDD:::::D    C:::::CCCCCCCC::::C
'  QQ::::::::::::::Q N::::::N       N:::::::ND:::::::::::::::DD      CC:::::::::::::::C
'    QQ:::::::::::Q  N::::::N        N::::::ND::::::::::::DDD          CCC::::::::::::C
'      QQQQQQQQ::::QQNNNNNNNN         NNNNNNNDDDDDDDDDDDDD                CCCCCCCCCCCCC
'              Q:::::Q                                                                 
'               QQQQQQ                                                                 
'**************************************************************************************                                                                                    
                                                                                     
                                                                                     
' global variables                                                     
common shared QNDC_CDRD_VENDOR_ID_0X748           as const long = 0x748
common shared QNDC_CDRD_PRODUCT_CODE_0X0A         as const long = 0x0a

common shared EC_TRQFBK_INDEX_6077h               as const long = 0x6077

' local variables
dim shared EC_RxPDO_1610                          as const long = 0x1610
dim shared EC_TxPDO_1A10                          as const long = 0x1A10
dim shared EC_TxPDO_1A11                          as const long = 0x1A11

'****************************************************************************
' Subroutine Name: EC_REMAP_QNDC_CDRD
' Description: PDO mapping for QNDC drive
' Called From: EC_SETUP.PRG
' Author: Rocky Qiu
' Input Parameters: drive_addr(long) - drive slave address
' Output Parameters: N/A
' Algorithm: 
' Revisions: v1.0.0 2021-05-06
'****************************************************************************
public sub EC_REMAP_QNDC_CDRD(byval drive_addr as long)
  
  ' Set Rx PDO Entries object
  call EC_SDO_WRITE(drive_addr, EC_SYNC_MANAGER_1C12, 0, 8, 0)
  call EC_SDO_WRITE(drive_addr, EC_SYNC_MANAGER_1C12, 1, 16, EC_RxPDO_1600)
  call EC_SDO_WRITE(drive_addr, EC_SYNC_MANAGER_1C12, 2, 16, EC_RxPDO_1610)
  ' Set number of entries
  call EC_SDO_WRITE(drive_addr, EC_SYNC_MANAGER_1C12, 0, 8, 2)

  '------------------------------- 0x1600 -------------------------------
  ' Zero number of entries
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1600, 0, 8, 0)

  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1600, 1, 32, 0x60400010) '-- Control Word
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1600, 2, 32, 0x607A0020) '-- Target Position
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1600, 3, 32, 0x60600008) '-- Modes of operation
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1600, 4, 32, 0x27FE0008) '-- Dummy Byte
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1600, 5, 32, 0x60B20010) '-- Torque Offset
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1600, 6, 32, 0x60710010) '-- Target Torque

  ' Set number of entries
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1600, 0, 8, 6)
  
  '------------------------------- 0x1610 --------------------------------
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1610, 0, 8, 0)

  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1610, 1, 32, 0x60400010 + 0x8000000) '-- Control Word  
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1610, 2, 32, 0x607A0020 + 0x8000000) '-- Target Position  
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1610, 3, 32, 0x60600008 + 0x8000000) '-- Modes of operation  
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1610, 4, 32, 0x27FE0008 + 0x8000000) '-- Dummy Byte 
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1610, 5, 32, 0x60B20010 + 0x8000000) '-- Torque Offset 
  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1610, 6, 32, 0x60710010 + 0x8000000) '-- Target Torque

  call EC_SDO_WRITE(drive_addr, EC_RxPDO_1610, 0, 8, 6)

  ' ******************************************************
  ' Set Tx PDO Entries object
  call EC_SDO_WRITE(drive_addr, EC_SYNC_MANAGER_1C13, 0, 8, 0)
  call EC_SDO_WRITE(drive_addr, EC_SYNC_MANAGER_1C13, 1, 16, EC_TxPDO_1A00)
  call EC_SDO_WRITE(drive_addr, EC_SYNC_MANAGER_1C13, 2, 16, EC_TxPDO_1A10)
  call EC_SDO_WRITE(drive_addr, EC_SYNC_MANAGER_1C13, 3, 16, EC_TxPDO_1A01)
  call EC_SDO_WRITE(drive_addr, EC_SYNC_MANAGER_1C13, 4, 16, EC_TxPDO_1A11)
  ' Set number of entries
  call EC_SDO_WRITE(drive_addr, EC_SYNC_MANAGER_1C13, 0, 8, 4)
  
'---------------------------------- 0x1A00 -------------------------------------------------
  ' Zero number of entries
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A00, 0, 8, 0)

  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A00, 1, 32, 0x60410010) '-- Status Word
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A00, 2, 32, 0x60640020) '-- Position Actual Value
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A00, 3, 32, 0x60770010) '-- Torque Actual Value
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A00, 4, 32, 0x60610008) '-- Mode of Operation Display
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A00, 5, 32, 0x27FF0008) '-- Dummy Byte

  ' Set number of entries
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A00, 0, 8, 5)
  
  '-------------------------------- 0x1A10 -----------------------------------------------------
  
  ' Zero number of entries
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A10, 0, 8, 0)

  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A10, 1, 32, 0x60410010 + 0x8000000) '-- Status Word
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A10, 2, 32, 0x60640020 + 0x8000000) '-- Position Actual Value
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A10, 3, 32, 0x60770010 + 0x8000000) '-- Torque Actual Value
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A10, 4, 32, 0x60610008 + 0x8000000) '-- Mode of Operation Display
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A10, 5, 32, 0x27FF0008 + 0x8000000) '-- Dummy Byte
  ' Set number of entries
  call EC_SDO_WRITE(drive_addr, 0x1A10, 0, 8, 5)

  '-------------------------0x1A01 ------------------------------------------
  ' Zero number of entries
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A01, 0, 8, 0)

  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A01, 1, 32, 0x603F0010) '-- Error Code
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A01, 2, 32, 0x60F40020) '-- Following Error Actual Value
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A01, 3, 32, 0x27040010) '-- warning code
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A01, 4, 32, 0x60FD0020) '-- Digital IO status

  ' Set number of entries
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A01, 0, 8, 4)
  '---------------------------------------------------------------------------
  '-------------------------0x1A11 ------------------------------------------
  ' Zero number of entries
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A11, 0, 8, 0)

  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A11, 1, 32, 0x603F0010 + 0x8000000) '-- Error Code
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A11, 2, 32, 0x60F40020 + 0x8000000) '-- Following Error Actual Value
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A11, 3, 32, 0x27040010 + 0x8000000) '-- warning code
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A11, 4, 32, 0x60FD0020 + 0x8000000) '-- Digital IO status

  ' Set number of entries
  call EC_SDO_WRITE(drive_addr, EC_TxPDO_1A11, 0, 8, 4)
  '---------------------------------------------------------------------------    

  EC_PDOAssignTotalNumber = 30 ' Must update this value after modifying PDO mapping

end sub 'EC_REMAP_QNDC_CDRD

'****************************************************************************
' Subroutine Name: EC_SET_QNDC_PDOS_CDRD
' Description: EC_INSTALL_QNDC_CDRD
' Author: Rocky Qiu
' Input Parameters: idx (long) - drive slave address
'                   busid (long) - BUS ID
' Output Parameters: N/A
' Algorithm: 
' Revisions: v1.0.0 2021-04-30
'****************************************************************************
public sub EC_SET_QNDC_PDOS_CDRD(byval idx as long, byval busid as long)

  dim i as long
  
    for i = 0 to 1

      call EC_SET_CTRL_OBJECT(busid, idx, EC_CTRL_INDEX_6040h + 0x800 * i, EC_CTRL_SUBINDEX_00h)
  
      call EC_SET_PCMD_OBJECT(busid,idx, EC_PCMD_INDEX_607Ah + 0x800 * i, EC_PCMD_SUBINDEX_00h)
      
      call EC_SET_STATUS_OBJECT(busid, idx, EC_STATUS_INDEX_6041h + 0x800 * i, EC_STATUS_SUBINDEX_00h)
      
      call EC_SET_PFB_OBJECT(busid, idx, EC_PFB_INDEX_6064h + 0x800 * i, EC_PFB_SUBINDEX_00h)
      
      call EC_SET_TRQCMD_OBJECT(busid, idx, EC_TRQCMD_INDEX_6071h + 0x800 * i, EC_TRQCMD_SUBINDEX_00h)
           
      call EC_SET_TRQFBK_OBJECT(busid, idx, EC_TRQFBK_INDEX_6077h + 0x800 * i, EC_TRQFBK_SUBINDEX_00h)
      
      call EC_SET_TRQADDCMD_OBJECT(busid, idx, EC_TRQADDCMD_INDEX_60B2h + 0x800 * i, EC_TRQADDCMD_SUBINDEX_00h)
      
      call EC_SET_PEXT_OBJECT(busid, idx, EC_PEXT_INDEX_60F4h + 0x800 * i, EC_PEXT_SUBINDEX_00h)

      call EC_SET_ERROR_OBJECT(busid, idx, EC_ERROR_INDEX_603Fh + 0x800 * i, EC_ERROR_SUBINDEX_00h)
      
      call EC_SET_WARNING_OBJECT(busid, idx, EC_WARNING_INDEX_2704h + 0x800 * i, EC_WARNING_INDEX_00h)

  next
  
end sub 'EC_SET_QNDC_PDOS_CDRD

'****************************************************************************
' Subroutine Name: EC_INSTALL_QNDC_CDRD
' Description: install CDRD drive
' Called From: EC_SETUP.PRG
' Author: Rocky Qiu
' Input Parameters: drive_addr(long) - drive slave address
' Output Parameters: N/A
' Algorithm: 
' Revisions: v1.0.0 2021-05-06
'****************************************************************************
public sub EC_INSTALL_QNDC_CDRD(byval drive_addr as long)

  dim lRetval   as long

  call EC_USE_SLAVE(drive_addr, EC_SLAVES_INFO[drive_addr]->vendor_id, EC_SLAVES_INFO[drive_addr]->product_code, "ECAT_AXIS_SLAVE", TRUE)  
  
  lRetVal = EC_SET_NEW_STATIC_DISTANCE(1000) 'set static time : 1000 us

  lRetval = EC_SLAVE_SET_OVERLAPPING_PDOS(drive_addr, TRUE) 'Slave allows overlapping 重叠pdos

  lRetval = EC_SLAVE_SET_AXES_NUMBER(drive_addr, 2) 'One slave with two axis

  call EC_SET_QNDC_PDOS_CDRD(drive_addr, EC_BUSID)


end sub 'EC_INSTALL_QNDC_CDRD